#include <iostream>
#include <vector>
#include <pcl/features/moment_of_inertia_estimation.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>          
#include <pcl/ModelCoefficients.h>
#include <pcl/segmentation/extract_clusters.h>
#include <Eigen/Core>
#include<math.h>
#include <pcl/console/time.h>

using namespace Eigen;
 

typedef pcl::PointXYZ PointT;

int
 main (int argc, char** argv)
{
  pcl::PCLPointCloud2::Ptr cloud_blob (new pcl::PCLPointCloud2), cloud_filtered_blob (new pcl::PCLPointCloud2);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>), cloud_ds (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p (new pcl::PointCloud<pcl::PointXYZ>),cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
  //pcl::NormalEstimation<PointT, pcl::Normal> ne;
  //pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
  pcl::ExtractIndices<pcl::Normal> extract_normals;
  //pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
  // Create the filtering object
  pcl::ExtractIndices<pcl::PointXYZ> extract;
  Eigen::Vector4f centroid;  //质心
  pcl::console::TicToc tt; 
  // Fill in the cloud data
  pcl::PCDReader reader;
  reader.read (argv[1], *cloud_blob);
  tt.tic();
  pcl::PCDWriter writer;

  //std::cerr << "PointCloud before filtering: " << cloud_blob->width * cloud_blob->height << " data points." << std::endl;
  //std::cerr << "height before filtering: " << cloud_blob->height << " data points." << std::endl;

  // Create the filtering object: downsample the dataset using a leaf size of 1cm
  pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
  sor.setInputCloud (cloud_blob);
  sor.setLeafSize (0.01f, 0.01f, 0.01f);
  sor.filter (*cloud_filtered_blob);

  // Convert to the templated PointCloud
  pcl::fromPCLPointCloud2 (*cloud_filtered_blob, *cloud_ds);
 // std::cerr << "PointCloud after downsampling: " << cloud_ds->width * cloud_ds->height << " data points." << std::endl;
 // std::cerr << "height after downsampling: " <<cloud_ds->height << " data points." << std::endl;
  
  pcl::PassThrough<pcl::PointXYZ> pass;
  pass.setInputCloud (cloud_ds);
  pass.setFilterFieldName ("z");
  pass.setFilterLimits (0.0, 1.0);
  //pass.setFilterLimitsNegative (true);
  pass.filter (*cloud_filtered);
  //std::cerr << "PointCloud after passthrough: " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl;
  //std::cerr << "height after passthrough: " <<  cloud_filtered->height << " data points." << std::endl;
  //writer.write<pcl::PointXYZ> ( "00.pcd", *cloud_filtered, false);

  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
  // Create the segmentation object
  pcl::SACSegmentation<pcl::PointXYZ> seg;
  // Optional
  seg.setOptimizeCoefficients (true);
  // Mandatory
  seg.setModelType (pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setMaxIterations (100);
  seg.setDistanceThreshold (0.01);
  
  int i = 0, nr_points = (int) cloud_filtered->points.size ();
  // While 30% of the original cloud is still there
  while (cloud_filtered->points.size () > 0.3 * nr_points)
  {
    // Segment the largest planar component from the remaining cloud
    seg.setInputCloud (cloud_filtered);
    seg.segment (*inliers, *coefficients);
    if (inliers->indices.size () == 0)
    {
      std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
      break;
    }

    // Extract the inliers
    extract.setInputCloud (cloud_filtered);
    extract.setIndices (inliers);
    extract.setNegative (false);
    extract.filter (*cloud_p);
    //std::cerr << "PointCloud representing the planar component: " << cloud_p->width * cloud_p->height << " data points." << std::endl;

    std::stringstream ss;
    ss << "table_scene_lms400_plane_" << i << ".pcd";
    //writer.write<pcl::PointXYZ> (ss.str (), *cloud_p, false);

    // Create the filtering object
    extract.setNegative (true);
    extract.filter (*cloud_f);
    cloud_filtered.swap (cloud_f);
    i++;
  }

 // writer.write<pcl::PointXYZ> ("table.pcd", *cloud_p, false);

  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>); 
  tree->setInputCloud (cloud_p); 
  std::vector<pcl::PointIndices> cluster_indices; 
  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec; //欧式聚类对象 
  ec.setClusterTolerance (0.02); // 设置近邻搜索的搜索半径为2cm 
  ec.setMinClusterSize (50); 
  ec.setMaxClusterSize (25000); 
  ec.setSearchMethod (tree); 
  ec.setInputCloud (cloud_p); 
  ec.extract (cluster_indices); 
  //分割出来的是点云索引 //分别访问每个聚类的循环 
  int j = 0; 
  for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it) 
  { 
    //迭代容器中的点云的索引，并且分开保存索引的点云 
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>); 
    for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
     //设置保存点云的属性问题 
      cloud_cluster->points.push_back (cloud_p->points[*pit]); //* 
    cloud_cluster->width = cloud_cluster->points.size (); 
    cloud_cluster->height = 1; 
    cloud_cluster->is_dense = true; 
    //std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl; 
    std::stringstream ss; ss << "cloud_cluster_" << j << ".pcd"; 
    //writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); //*

    pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor; 
    feature_extractor.setInputCloud (cloud_cluster); 
    feature_extractor.compute (); 
    std::vector <float> moment_of_inertia; 
    std::vector <float> eccentricity;
    pcl::PointXYZ min_point_AABB; pcl::PointXYZ max_point_AABB; 
    pcl::PointXYZ min_point_OBB; pcl::PointXYZ max_point_OBB; 
    pcl::PointXYZ position_OBB; Eigen::Matrix3f rotational_matrix_OBB; 
    float major_value, middle_value, minor_value; 
    Eigen::Vector3f major_vector, middle_vector, minor_vector; 
    Eigen::Vector3f mass_center;
    float anglecos,angle; 
    feature_extractor.getMomentOfInertia (moment_of_inertia); 
    feature_extractor.getEccentricity (eccentricity); 
    feature_extractor.getAABB (min_point_AABB, max_point_AABB); 
    feature_extractor.getOBB (min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB); 
    feature_extractor.getEigenValues (major_value, middle_value, minor_value); 
    feature_extractor.getEigenVectors (major_vector, middle_vector, minor_vector); 
    feature_extractor.getMassCenter (mass_center); 
    pcl::visualization::PCLVisualizer viewer ("Matrix transformation example");
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transformed_cloud_color_handler (cloud_p, 230, 20, 20); // Red
    viewer.addPointCloud (cloud_p, transformed_cloud_color_handler, "transformed_cloud");

    viewer.addCoordinateSystem (1.0, "cloud", 0);
    viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey
    viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_cloud");
    viewer.setPosition(800, 400); 
    std::cout <<"点云质心是（" << mass_center[0] << "," << mass_center[1] << "," << mass_center[2] << ")." <<std::endl; 
    std::cout <<"点云的曲面法线是(" << minor_vector[0] << "," << minor_vector[1] << "," << minor_vector[2] << ")." <<std::endl;
    anglecos=(-mass_center[0]*minor_vector[0]-mass_center[1]*minor_vector[1]-mass_center[2]*minor_vector[2])/(sqrt(mass_center[0]*mass_center[0]+mass_center[1]*mass_center[1]+mass_center[2]*mass_center[2])*sqrt(minor_vector[0]*minor_vector[0]+minor_vector[1]*minor_vector[1]+minor_vector[2]*minor_vector[2]));
    angle=acos(anglecos)*180/3.1415926;
    std::cout <<"angle:"  << angle  <<std::endl;
    Eigen::Vector3f position (position_OBB.x, position_OBB.y, position_OBB.z); 
    Eigen::Quaternionf quat (rotational_matrix_OBB); 
    viewer.addCube (position, quat, max_point_OBB.x - min_point_OBB.x, max_point_OBB.y - min_point_OBB.y, max_point_OBB.z - min_point_OBB.z, "OBB"); 
    pcl::PointXYZ center (mass_center (0), mass_center (1), mass_center (2)); 
    pcl::PointXYZ x_axis (major_vector (0) + mass_center (0), major_vector (1) + mass_center (1), major_vector (2) + mass_center (2)); 
    pcl::PointXYZ y_axis (middle_vector (0) + mass_center (0), middle_vector (1) + mass_center (1), middle_vector (2) + mass_center (2)); 
    pcl::PointXYZ z_axis (minor_vector (0) + mass_center (0), minor_vector (1) + mass_center (1), minor_vector (2) + mass_center (2)); 
    viewer.addLine (center, x_axis, 1.0f, 0.0f, 0.0f, "major eigen vector"); 
    viewer.addLine (center, y_axis, 0.0f, 1.0f, 0.0f, "middle eigen vector"); 
    viewer.addLine (center, z_axis, 0.0f, 0.0f, 1.0f, "minor eigen vector"); 
    std::cout <<argv[1] <<" 第" << j+1<<"个弹药箱，X：" << mass_center[0] << ",Z：" << mass_center[2] << ",angle：" <<angle<<",time:"<<tt.toc()<<"ms\n"<<std::endl; 
    while(!viewer.wasStopped()) 
    { 
       viewer.spinOnce ();
    } 
    tt.tic();
    j++;
  }

  return (0);
}

